/**************************************************************************************************
* Title: SumVector3_16f.cpp
* Author: Gael Huber
* Description: Implements 16-byte aligned 3D vector functions.
**************************************************************************************************/
#include "SumMath.h"

/**************************************************************************************************
* Hermite interpolation between position V1, tangent T1 (when s == 0) and position V2, tangent T2
* (when s == 1).
**************************************************************************************************/
Vector3_16f* Vec3Hermite_16f(Vector3_16f* pOut, const Vector3_16f* pV1, const Vector3_16f* pT1, 
	const Vector3_16f* pV2, const Vector3_16f* pT2, float s) {
	// Calculate a, b, c, and d values for formula
	float a(2.0f*s*s*s - 3.0f*s*s + 1.0f);
	float b(-2.0f*s*s*s + 3.0f*s*s);
	float c(s*s*s - 2.0f*s*s + s);
	float d(s*s*s - s*s);

	// Solve for a*v1 + b*v2 + c*t1 + d*t2
	Vector3_16f v1, v2;

	Vec3Scale_16f(&v1, pV1, a);
	Vec3Scale_16f(&v2, pV2, b);
	Vec3Add_16f(pOut, &v1, &v2);

	Vec3Scale_16f(&v1, pT1, c);
	Vec3Add_16f(pOut, pOut, &v1);

	Vec3Scale_16f(&v1, pT2, d);
	Vec3Add_16f(pOut, pOut, &v1);

	return pOut;
}

/**************************************************************************************************
* CatmullRom interpolation between V1 (when s == 0) and V2 (when s == 1)
**************************************************************************************************/
Vector3_16f* Vec3CatmullRom_16f(Vector3_16f* pOut, const Vector3_16f* pV0, const Vector3_16f* pV1,
	const Vector3_16f* pV2, const Vector3_16f* pV3, float s) {
	// Calculate a, b, c, d
	float a(-s*s*s + 2.0f*s*s - s);
	float b(3.0f*s*s*s - 5.0f*s*s + 2.0f);
	float c(-3.0f*s*s*s + 4.0f*s*s + s);
	float d(s*s*s - s*s);

	// Solve for (a*p1 + b*p2 + c*p3 + d*p4)/2
	Vector3_16f v1, v2;

	Vec3Scale_16f(&v1, pV0, a);
	Vec3Scale_16f(&v2, pV1, b);
	Vec3Add_16f(pOut, &v1, &v2);

	Vec3Scale_16f(&v1, pV2, c);
	Vec3Add_16f(pOut, pOut, &v1);

	Vec3Scale_16f(&v1, pV3, d);
	Vec3Add_16f(pOut, pOut, &v1);

	Vec3Scale_16f(pOut, pOut, 0.5f);

	return pOut;
}

/**************************************************************************************************
* Barycentric coordinates. V1 + f(V2 - V1) + g(V3 - V1)
**************************************************************************************************/
Vector3_16f* Vec3Barycentric_16f(Vector3_16f* pOut, const Vector3_16f* pV1, const Vector3_16f* pV2, 
	const Vector3_16f* pV3,	float f, float g) {
	pOut->x = pV1->x + f * (pV2->x - pV1->x) + g * (pV3->x - pV1->x);
	pOut->y = pV1->y + f * (pV2->y - pV1->y) + g * (pV3->y - pV1->y);
	pOut->z = pV1->z + f * (pV2->z - pV1->z) + g * (pV3->z - pV1->z);
	return pOut;
}

/**************************************************************************************************
* Transform by matrix (x, y, z, 1)
**************************************************************************************************/
Vector4_16f* Vec3Transform_16f(Vector4_16f* pOut, const Vector3_16f* pV, const Matrix_16f* pM) {
	__m128 m(_mm_mul_ps(pV->v, pM->v[0]));
	pOut->x = m.m128_f32[0] + m.m128_f32[1] + m.m128_f32[2] + pM->_14;

	m = _mm_mul_ps(pV->v, pM->v[1]);
	pOut->y = m.m128_f32[0] + m.m128_f32[1] + m.m128_f32[2] + pM->_24;

	m = _mm_mul_ps(pV->v, pM->v[2]);
	pOut->z = m.m128_f32[0] + m.m128_f32[1] + m.m128_f32[2] + pM->_34;

	m = _mm_mul_ps(pV->v, pM->v[3]);
	pOut->w = m.m128_f32[0] + m.m128_f32[1] + m.m128_f32[2] + pM->_44;

	return pOut;
}

/**************************************************************************************************
* Transform coord by matrix (x, y, z, 1)
**************************************************************************************************/
Vector3_16f* Vec3TransformCoord_16f(Vector3_16f* pOut, const Vector3_16f* pV, const Matrix_16f* pM) {
	__m128 m(_mm_mul_ps(pV->v, pM->v[0]));
	pOut->x = m.m128_f32[0] + m.m128_f32[1] + m.m128_f32[2] + pM->_14;

	m = _mm_mul_ps(pV->v, pM->v[1]);
	pOut->y = m.m128_f32[0] + m.m128_f32[1] + m.m128_f32[2] + pM->_24;

	m = _mm_mul_ps(pV->v, pM->v[2]);
	pOut->z = m.m128_f32[0] + m.m128_f32[1] + m.m128_f32[2] + pM->_34;

	return pOut;
}

/**************************************************************************************************
* Transform normal by matix (x, y, z, 0)
**************************************************************************************************/
Vector3_16f* Vec3TransformNormal_16f(Vector3_16f* pOut, const Vector3_16f* pV, const Matrix_16f* pM) {
	__m128 m(_mm_mul_ps(pV->v, pM->v[0]));
	pOut->x = m.m128_f32[0] + m.m128_f32[1] + m.m128_f32[2];

	m = _mm_mul_ps(pV->v, pM->v[1]);
	pOut->y = m.m128_f32[0] + m.m128_f32[1] + m.m128_f32[2];

	m = _mm_mul_ps(pV->v, pM->v[2]);
	pOut->z = m.m128_f32[0] + m.m128_f32[1] + m.m128_f32[2];
	
	return pOut;
}

/**************************************************************************************************
* Project from object space into screen space
**************************************************************************************************/
Vector3_16f* Vec3Project_16f(Vector3_16f* pOut, const Vector3_16f* pV, const Viewport* pViewport, 
	const Matrix_16f* pProjection, const Matrix_16f* pView, const Matrix_16f* pWorld) {
	// Create world transform
	Vector3_16f vW;
	Vec3TransformCoord_16f(&vW, pV, pWorld);

	// Construct the view transform
	Vector3_16f vV;
	Vec3TransformCoord_16f(&vV, &vW, pView);

	// Construct the projection transform
	Vector3_16f vP;
	Vec3TransformCoord_16f(&vP, &vV, pProjection);

	// Projection
	pOut->x = pViewport->x + (1.0f + vP.x) * pViewport->width * 0.5f;
	pOut->y = pViewport->y + (1.0f - vP.y) * pViewport->height * 0.5f;
	pOut->z = pViewport->minZ + vP.z * (pViewport->maxZ - pViewport->minZ);

	return pOut;
}

/**************************************************************************************************
* Project from screen space into object space
**************************************************************************************************/
Vector3_16f* Vec3Unproject_16f(Vector3_16f* pOut, const Vector3_16f* pV, const Viewport* pViewport, 
	const Matrix_16f* pProjection, const Matrix_16f* pView, const Matrix_16f* pWorld) {
	// Infer post-transform coordinates from projected matrix
	Vector3_16f vP;
	vP.x = (pV->x - (float)pViewport->x) * 2.0f / (float)pViewport->width - 1.0f;
	vP.y = (pV->y - (float)pViewport->y) * -2.0f / (float)pViewport->height + 1.0f;
	vP.z = (pV->z - pViewport->minZ) / (pViewport->maxZ - pViewport->minZ);

	// Transform by inverse of projection matrix
	Matrix_16f mPI;
	MatrixInverse_16f(&mPI, 0, pProjection);
	Vector3_16f vV;
	Vec3TransformCoord_16f(&vV, &vP, &mPI);

	// Transform by inverse of view matrix
	Matrix_16f mVI;
	MatrixInverse_16f(&mVI, 0, pView);
	Vector3_16f vW;
	Vec3TransformCoord_16f(&vW, &vV, &mVI);

	// Transform by inverse of world matrix
	Matrix_16f mWI;
	MatrixInverse_16f(&mWI, 0, pWorld);
	Vec3TransformCoord_16f(pOut, &vW, &mWI);

	return pOut;
}
